import sensor, image, time, json # type: ignore

# 最小二乘法
def least_squares(x, y):
    n = len(x)
    sum_x = sum(x)
    sum_y = sum(y)
    sum_xy = 0
    for x_temp,y_temp in zip(x,y):
        sum_xy += x_temp * y_temp
    sum_y2 = 0
    for y_temp in y:
        sum_y2 += y_temp ** 2
    a = (n * sum_xy - sum_x * sum_y) / (n * sum_y2 - sum_y ** 2)
    b = (sum_x - a * sum_y) / n
    return a, b

# 限制x的大小
def limit(x,a,b):
    if (x < a):
        x = a
    elif (x > b):
        x = b
    return x

# 避免摄像头高度造成位移变化
def position_limit(position):
    position = limit(position,1,160) # 限幅
    return position

# 获取色差
def get_chromatism(img,a,b):
    a_state = img.get_statistics(roi = a.rect())
    b_state = img.get_statistics(roi = b.rect())
    l_theta = a_state.l_mean() - b_state.l_mean()
    a_theta = a_state.a_mean() - b_state.a_mean()
    b_theta = a_state.b_mean() - b_state.b_mean()
    return (l_theta ** 2 + a_theta ** 2 + b_theta ** 2)**0.5

# 通过四个点查找交点
def Get_Point(x,y):
    try:
        x0 = ((x[2] - x[3]) * (x[1] * y[0] - x[0] * y[1]) - (x[0] - x[1]) * (x[3] * y[2] - x[2] * y[3])) / ((x[2] - x[3]) * (y[0] - y[1]) - (x[0] - x[1]) * (y[2] - y[3]))
        y0 = ((y[2] - y[3]) * (y[1] * x[0] - y[0] * x[1]) - (y[0] - y[1]) * (y[3] * x[2] - y[2] * x[3])) / ((y[2] - y[3]) * (x[0] - x[1]) - (y[0] - y[1]) * (x[2] - x[3]))
    except ZeroDivisionError:
        print("0")
        return 0,0
    return x0,y0

